﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Diagnostics;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningComponents;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;
using System.Text;

namespace MotionPlanning.MotionPlanningAlgorithms.kbestRRT_Prototype
{
    class kbestRRT : Planner_RRTBased
    {
        public override Node SpawnNode_Like(double[] q)
        {
            if (q == null)
                throw new Exception("Input q cannot be null");
            int DOF_state = this.System.Get_DOF_state();
            double[] q_new = new double[DOF_state];
            for (int i = 0; i < DOF_state; ++i)
                q_new[i] = q[i];
            Node x = Node.SpawnGraphicNode();
            x.set_Value(new NodeData_kbestRRT(q_new, 0));
            return x;
        }
        public override Node SpawnNode_As(double[] q)
        {
            if (q.Length != this.System.Get_DOF_state())
                throw new Exception("q.length != DOF_state. in RRT.SpawnNode_As");
            Node x = Node.SpawnGraphicNode();
            x.set_Value(new NodeData_kbestRRT(q, 0));
            return x;
        }
        public override Edge SpawnEdge()
        {
            Edge e = Edge.SpawnGraphicEdge();
            e.set_Value(new EdgeData_kbestRRT());
            return e;
        }
        public override Edge SpawnEdge(Node entry, Node exit)
        {
            Edge e = Edge.SpawnGraphicEdge(entry, exit);
            e.set_Value(new EdgeData_kbestRRT());
            return e;
        }
        public Edge SpawnEdge(Node entry, Node exit, double c_inc, double[] ctrl_inc)
        {
            Edge e = Edge.SpawnGraphicEdge(entry, exit);
            e.set_Value(new EdgeData_kbestRRT(c_inc, ctrl_inc));
            return e;
        }

        public kbestRRT(RoboticSystemComponent system, VisualizationComponent visualizer, string motionPlanningConfigFile)
            : base(system, visualizer, motionPlanningConfigFile)
        {
        }

        public override void Execute(int n_iterations)
        {
            if (x_root == null)
                throw new Exception("x_root uninitialized. Forget to call SetRoot()?");

            int iterations_this_round = 0;
            while (iterations_this_round < n_iterations)
            {
                ++iterations_this_round;
                ++iterations_total;
                /*
                 ----------------- Sampling Phase -------------------
                 */
                double[] q_sample = this.Sampler.NextQ(SamplingComponent.SamplingMode.SAMPLING, this.System);
                /*
                 ---------- Nearest Neighbor Search Phase -----------
                 */
                double dist_nearest = double.MaxValue;
                Object obj_nearest = null;
                this.Vertices.NearestNeighbor_Approximate(q_sample, ref dist_nearest, ref obj_nearest);
                if (obj_nearest == null) continue;
                Node x_nearest = (Node)obj_nearest;
                /*
                 ----------------- Extension Phase ------------------
                 */

                //boosting
                bool doBoosting = true;
                if (doBoosting == true)
                {
                    int K = (int)(1 + Math.Log(this.Vertices.GetSize(), Math.E));
                    List<object> X_kNearest = null;
                    this.Vertices.NearestNeighbors_KClosest_Exclusive(q_sample, K, ref X_kNearest);
                    double v_min = double.MaxValue;
                    Node x_min = null;
                    for (int i = 0; i < X_kNearest.Count; ++i)
                    {
                        Node x_cur = (Node)X_kNearest[i];
                        double v_cur = x_cur.get_Value().get_utility_c();
                        if (v_cur < v_min)
                        {
                            v_min = v_cur;
                            x_min = x_cur;
                        }
                    }
                    x_nearest = x_min;
                }

                //propagation
                double[] u_inc = new double[this.System.Get_DOF_control()];
                u_inc = this.Sampler.NextControl(SamplingComponent.SamplingMode.CONTROL, this.System);
                double t_inc = u_inc[u_inc.Length - 1];

                double[] q_new = new double[this.System.Get_DOF_state()];
                double cost_inc = 0;
                bool succ_propagation = this.System.Propagate(x_nearest.get_Value().get_q(), u_inc, t_inc, ref q_new, ref cost_inc);
                if (succ_propagation == false) continue;
                bool inRange = this.System.PointInRange(q_new);
                if (inRange == false) continue;

                //grow tree
                Node x_new = this.SpawnNode_As(q_new);
                x_new.get_Value().set_utility_c(x_nearest.get_Value().get_utility_c() + cost_inc);
                Edge e_new = this.SpawnEdge(x_nearest, x_new, cost_inc, u_inc);
                x_new.add_ParentEdge(e_new);
                x_nearest.add_ChildEdge(e_new);
                this.Vertices.AddPoint(x_new.get_Value().get_q(), x_new);

                //goal check
                bool DoGoalCheck = false;
                if (DoGoalCheck)
                {
                    if (GoalReached(x_new.get_Value().get_q()))
                    {
                        if (this.solution != null)
                        {
                            if (x_new.get_Value().get_utility_c() < this.solution.GetCost())
                                this.solution = new Trajectory(x_new);
                        }
                        else
                            this.solution = new Trajectory(x_new);
                    }
                }

                //miscellaneous
                bool DoMiscellaneous = false;
                if (DoMiscellaneous)
                {
                    this.incrementalNodeArray.Add(x_new);
                    if (this.incrementalNodeArray.Count > 1000)
                    {
                        throw new NotImplementedException();
                        //this.Plot_incrementally();
                        //Thread.Sleep(0);
                    }
                }
            }
        }

        public override string ToString()
        {
            throw new NotImplementedException();
        }

        public override void Test()
        {
            throw new NotImplementedException();
        }

        public override bool GoalReached(double[] q_test)
        {
            throw new NotImplementedException();
        }

    }
}
